#include "GWMatrix.h"
#include <cstring>
#include <math.h>

const Matrix4 Matrix4::IDENTITY =
	Matrix4(1.f, 0.f, 0.f, 0.f,
			0.f, 1.f, 0.f, 0.f,
			0.f, 0.f, 1.f, 0.f,
			0.f, 0.f, 0.f, 1.f);

const Matrix4& Matrix4::operator=( const Matrix4& rhs )
{
	memcpy( this, &rhs.m11, sizeof(Matrix4) );
	return *this;
}

Matrix4 Matrix4::operator*( const Matrix4& rhs ) const
{
	Matrix4 r;

	r.m11 = m11 * rhs.m11 + m12 * rhs.m21 + m13 * rhs.m31 + m14 * rhs.m41;
	r.m12 = m11 * rhs.m12 + m12 * rhs.m22 + m13 * rhs.m32 + m14 * rhs.m42;
	r.m13 = m11 * rhs.m13 + m12 * rhs.m23 + m13 * rhs.m33 + m14 * rhs.m43;
	r.m14 = m11 * rhs.m14 + m12 * rhs.m24 + m13 * rhs.m34 + m14 * rhs.m44;

	r.m21 = m21 * rhs.m11 + m22 * rhs.m21 + m23 * rhs.m31 + m24 * rhs.m41;
	r.m22 = m21 * rhs.m12 + m22 * rhs.m22 + m23 * rhs.m32 + m24 * rhs.m42;
	r.m23 = m21 * rhs.m13 + m22 * rhs.m23 + m23 * rhs.m33 + m24 * rhs.m43;
	r.m24 = m21 * rhs.m14 + m22 * rhs.m24 + m23 * rhs.m34 + m24 * rhs.m44;

	r.m31 = m31 * rhs.m11 + m32 * rhs.m21 + m33 * rhs.m31 + m34 * rhs.m41;
	r.m32 = m31 * rhs.m12 + m32 * rhs.m22 + m33 * rhs.m32 + m34 * rhs.m42;
	r.m33 = m31 * rhs.m13 + m32 * rhs.m23 + m33 * rhs.m33 + m34 * rhs.m43;
	r.m34 = m31 * rhs.m14 + m32 * rhs.m24 + m33 * rhs.m34 + m34 * rhs.m44;

	r.m41 = m41 * rhs.m11 + m42 * rhs.m21 + m43 * rhs.m31 + m44 * rhs.m41;
	r.m42 = m41 * rhs.m12 + m42 * rhs.m22 + m43 * rhs.m32 + m44 * rhs.m42;
	r.m43 = m41 * rhs.m13 + m42 * rhs.m23 + m43 * rhs.m33 + m44 * rhs.m43;
	r.m44 = m41 * rhs.m14 + m42 * rhs.m24 + m43 * rhs.m34 + m44 * rhs.m44;

	return r;
}

void Matrix4::MakeIdentity()
{
	memset(this, 0, sizeof(Matrix4));
	m11 = m22 = m33 = m44 = 1.f;
}

Matrix4 Matrix4::InverseMatrix() const
{
	float v0 = m31 * m42 - m32 * m41;
	float v1 = m31 * m43 - m33 * m41;
	float v2 = m31 * m44 - m34 * m41;
	float v3 = m32 * m43 - m33 * m42;
	float v4 = m32 * m44 - m34 * m42;
	float v5 = m33 * m44 - m34 * m43;

	float t00 = + (v5 * m22 - v4 * m23 + v3 * m24);
	float t10 = - (v5 * m21 - v2 * m23 + v1 * m24);
	float t20 = + (v4 * m21 - v2 * m22 + v0 * m24);
	float t30 = - (v3 * m21 - v1 * m22 + v0 * m23);

	float invDet = 1 / (t00 * m11 + t10 * m12 + t20 * m13 + t30 * m14);

	float d00 = t00 * invDet;
	float d10 = t10 * invDet;
	float d20 = t20 * invDet;
	float d30 = t30 * invDet;

	float d01 = - (v5 * m12 - v4 * m13 + v3 * m14) * invDet;
	float d11 = + (v5 * m11 - v2 * m13 + v1 * m14) * invDet;
	float d21 = - (v4 * m11 - v2 * m12 + v0 * m14) * invDet;
	float d31 = + (v3 * m11 - v1 * m12 + v0 * m13) * invDet;

	v0 = m21 * m42 - m22 * m41;
	v1 = m21 * m43 - m23 * m41;
	v2 = m21 * m44 - m24 * m41;
	v3 = m22 * m43 - m23 * m42;
	v4 = m22 * m44 - m24 * m42;
	v5 = m23 * m44 - m24 * m43;

	float d02 = + (v5 * m12 - v4 * m13 + v3 * m14) * invDet;
	float d12 = - (v5 * m11 - v2 * m13 + v1 * m14) * invDet;
	float d22 = + (v4 * m11 - v2 * m12 + v0 * m14) * invDet;
	float d32 = - (v3 * m11 - v1 * m12 + v0 * m13) * invDet;

	v0 = m32 * m21 - m31 * m22;
	v1 = m33 * m21 - m31 * m23;
	v2 = m34 * m21 - m31 * m24;
	v3 = m33 * m22 - m32 * m23;
	v4 = m34 * m22 - m32 * m24;
	v5 = m34 * m23 - m33 * m24;

	float d03 = - (v5 * m12 - v4 * m13 + v3 * m14) * invDet;
	float d13 = + (v5 * m11 - v2 * m13 + v1 * m14) * invDet;
	float d23 = - (v4 * m11 - v2 * m12 + v0 * m14) * invDet;
	float d33 = + (v3 * m11 - v1 * m12 + v0 * m13) * invDet;

	return Matrix4(
		d00, d01, d02, d03,
		d10, d11, d12, d13,
		d20, d21, d22, d23,
		d30, d31, d32, d33);
}

Matrix4 Matrix4::BuildRotationMatrix( float rad, ROTATION_AXIS axis )
{
	switch ( axis)
	{
	case ROTATION_AXIS_X:
		return Matrix4( 1.f, 0.f, 0.f, 0.f,
						0.f, cosf(rad),	-sinf(rad), 0.f,
						0.f, sinf(rad), cosf(rad), 0.f,
						0.f, 0.f, 0.f, 1.f );
	case ROTATION_AXIS_Y:
		return Matrix4( cosf(rad), 0.f, sinf(rad), 0.f,
						0.f, 1.f, 0.f, 0.f,
						-sinf(rad), 0.f, cosf(rad), 0.f,
						0.f, 0.f, 0.f, 1.f );
	case ROTATION_AXIS_Z:
		return Matrix4( cosf(rad), -sinf(rad), 0.f, 0.f,
						sinf(rad), cosf(rad), 0.f, 0.f,
						0.f, 0.f, 1.f, 0.f,
						0.f, 0.f, 0.f, 1.f );
	}

	return Matrix4::IDENTITY;
}

Matrix4 Matrix4::BuildTranslationMatrix( const Vector3& trans )
{
	return Matrix4( 1.f, 0.f, 0.f, 0.f,
					0.f, 1.f, 0.f, 0.f,
					0.f, 0.f, 1.f, 0.f,
					trans.x, trans.y, trans.z, 1.f );
}

Vector3 Vec3_Transform( const Vector3& vec, const Matrix4& mat )
{
	float x, y, z, w;

	x = vec.x * mat.m11 + vec.y * mat.m21 + vec.z * mat.m31 + 1.f * mat.m41;
	y = vec.x * mat.m12 + vec.y * mat.m22 + vec.z * mat.m32 + 1.f * mat.m42;
	z = vec.x * mat.m13 + vec.y * mat.m23 + vec.z * mat.m33 + 1.f * mat.m43;
	w = vec.x * mat.m14 + vec.y * mat.m24 + vec.z * mat.m34 + 1.f * mat.m44;

	return Vector3(x, y, z);
}

